#pragma once
#ifndef EXPENDED_TREE_H
#define EXPENDED_TREE_H

#include "basic_typedef.h"
#include "CollisionDetector.h"
#include <CGAL/Cartesian.h>
#include <iostream>
#include "Kd_tree_d.h"
#include "Graph.h"
#include "Oracle.h"

#define PI 3.14159265

class ExpendedTree
{
public:
	ExpendedTree(Point_d start_point, Oracle* oracle, MultiRobotsSampler* multi_robots_sampler);
	~ExpendedTree() {};

public:
	void expand(int num_of_iterations);
	int get_point_index(Point_d p);

public:
	Kd_tree_d<Kernel_d>* _kd_tree;
	Graph<int>* _tree;
	Point_d _start_point;
	Oracle* _oracle;
	vector<Point_d>	_all_tree_points;
	MultiRobotsSampler* _multi_robots_sampler;
};

#endif